﻿// -------------------------------------------------------------------------
//    @FileName         :    NFNavmeshUtil.cpp
//    @Author           :    gaoyi
//    @Date             :    2025-03-24
//    @Email			:    445267987@qq.com
//    @Module           :    NFNavmeshUtil
//
// -------------------------------------------------------------------------

#include "NFNavmeshUtil.h"
#include "NFComm/NFCore/NFFileUtility.h"
#include "NFComm/NFCore/NFStringUtility.h"
#include "NFComm/NFCore/NFPlatform.h"
#include "NFComm/NFPluginModule/NFStackTrace.h"
#include "NFComm/NFPluginModule/NFLogMgr.h"
#include "NFComm/NFPluginModule/NFCheck.h"

#include "Detour/Include/DetourNavMesh.h"
#include "Detour/Include/DetourNavMeshQuery.h"
#include "Detour/Include/DetourNavMeshBuilder.h"

#include "RecastDemo/Include/MeshHeaderDef.h"
#include "Detour/Include/DetourCommon.h"

#include "Frame/InputGeom.h"
#include "Frame/ChunkyTriMesh.h"
#include "Frame/MeshLoaderObj.h"
#include "Frame/NavRunContext.h"

#include "ParseUtil.h"

int NFNavmeshUtil::MakeNavmesh(const char* path, const char* filename)
{
    LOGSVR_TRACE("--begin-- path:" << path << ", filename:" << filename);

    CHECK_NULL(0, filename);
    std::string szResName = NFFileUtility::GetFileNameWithoutExt(filename);
    std::string full_name = NF_FORMAT("{}/scene/{}", path, filename);
    tinyxml2::XMLDocument* myDocument = new tinyxml2::XMLDocument();
    tinyxml2::XMLError error = myDocument->LoadFile(full_name.c_str());
    CHECK_EXPR(error == 0, error, "load file error={},filename={}", error, full_name);
    tinyxml2::XMLElement* pRootElement = myDocument->RootElement();

    int iRet = 0;

    tinyxml2::XMLElement* pElement = pRootElement->FirstChildElement();
    for (; pElement && 0 == iRet; pElement = pElement->NextSiblingElement())
    {
        if (0 == strcmp(pElement->Value(), "NavMesh"))
        {
            iRet = NFNavmeshUtil::ReadElementNavmeshData(pElement, path, szResName.c_str());
            break;
        }
    }

    LOGSVR_TRACE("--end-- path:" << path << ", filename:" << filename);

    return iRet;
}

int NFNavmeshUtil::ReadElementNavmeshData(tinyxml2::XMLElement* pElement, const char* pszPath, const char* pszResName)
{
    LOGSVR_TRACE("--begin-- path:" << pszPath << ", resname:" << pszResName);

    CHECK_NULL(0, pszPath);
    CHECK_NULL(0, pszResName);
    int iRet = 0;
    tinyxml2::XMLElement* pNavMeshElement = pElement;
    tinyxml2::XMLElement* pVertexElement = pNavMeshElement->FirstChildElement();
    tinyxml2::XMLElement* pTrigIndexElement = pVertexElement->NextSiblingElement();
    tinyxml2::XMLElement* pOffMeshLinksElement = pTrigIndexElement->NextSiblingElement();

    const int iMaxBuffSize = 1024 * 1024 * 9;
    char szAllocBuffer[iMaxBuffSize];
    memset(szAllocBuffer, 0, sizeof(szAllocBuffer));
    int vertext_num = 0, trig_num = 0;

    std::string szObjName = NF_FORMAT("{}/scene/navmesh/obj/{}.obj", pszPath, pszResName);
    iRet = ClientNavData2ServerObjData(szAllocBuffer, iMaxBuffSize, vertext_num, trig_num, pVertexElement, pTrigIndexElement, szObjName.c_str());
    CHECK_EXPR(iRet == 0, -1, "navmesh data make to obj data wrong. map={}", pszResName);
    if (vertext_num == 0 || trig_num == 0)
    {
        LOGSVR_ERROR("vertext == 0 || trig == 0 for map = " << pszResName);
        return -1;
    }

    InputGeom geom;
    if (false == geom.loadMesh(szAllocBuffer, strlen(szAllocBuffer)))
    {
        return -1;
    }

    CNavRunContext ctx;

    InputGeom* m_geom = &geom;
    const float* bmin = m_geom->getMeshBoundsMin();
    const float* bmax = m_geom->getMeshBoundsMax();
    const float* verts = m_geom->getMesh()->getVerts();
    const int nverts = m_geom->getMesh()->getVertCount();
    const int* tris = m_geom->getMesh()->getTris();
    const int ntris = m_geom->getMesh()->getTriCount();

    enum SamplePolyFlags
    {
        SAMPLE_POLYFLAGS_WALK = 0x01, // Ability to walk (ground, grass, road)
        SAMPLE_POLYFLAGS_SWIM = 0x02, // Ability to swim (water).
        SAMPLE_POLYFLAGS_DOOR = 0x04, // Ability to move through doors.
        SAMPLE_POLYFLAGS_JUMP = 0x08, // Ability to jump.
        SAMPLE_POLYFLAGS_DISABLED = 0x10, // Disabled polygon
        SAMPLE_POLYFLAGS_ALL = 0xffff // All abilities.
    };

    if (pOffMeshLinksElement)
    {
        tinyxml2::XMLElement* pChildElement = pOffMeshLinksElement->FirstChildElement();
        for (; pChildElement;
               pChildElement = pChildElement->NextSiblingElement())
        {
            tinyxml2::XMLElement* pAttrElement = pChildElement->FirstChildElement();
            const char* pValue = pAttrElement->GetText();
            CHECK_NULL(0, pValue);
            float start[3];
            int iaSize = 0;
            int iRet = CParseUtil::ParseStringToFloatArray(pValue, ",", start, (int)ARRAYSIZE(start), iaSize);
            CHECK_RET(iRet, "");

            pAttrElement = pAttrElement->NextSiblingElement();
            pValue = pAttrElement->GetText();
            CHECK_NULL(0, pValue);
            float end[3];
            iRet = CParseUtil::ParseStringToFloatArray(pValue, ",", end, (int)ARRAYSIZE(end), iaSize);
            CHECK_RET(iRet, "");

            pAttrElement = pAttrElement->NextSiblingElement();
            pValue = pAttrElement->GetText(); //Attribute("radius");
            CHECK_NULL(0, pValue);
            float rad = atof(pValue); // * 3;

            //linktype skip
            pAttrElement = pAttrElement->NextSiblingElement();

            pAttrElement = pAttrElement->NextSiblingElement();
            pValue = pAttrElement->GetText();
            CHECK_NULL(0, pValue);
            int area = atoi(pValue);

            pAttrElement = pAttrElement->NextSiblingElement();
            pValue = pAttrElement->GetText();
            //pValue = pChildElement->Attribute("linkDirection");
            CHECK_NULL(0, pValue);
            int bidir = atoi(pValue);

            LOGSVR_DEBUG("add offmesh connection, start[" << start[0] << ", " << start[1] << ", " << start[2] << "], end["
                << end[0] << ", " << end[1] << ", " << end[2] << "], rad=" << rad << ", area=" << area << ", dir=" << bidir << ".");
            m_geom->addOffMeshConnection(start, end, rad, bidir, area, SAMPLE_POLYFLAGS_WALK | SAMPLE_POLYFLAGS_JUMP);
        }
    }

    rcConfig m_cfg;
    memset(&m_cfg, 0, sizeof(m_cfg));
    m_cfg.cs = 0.3f;
    m_cfg.ch = 0.2f;
    m_cfg.walkableSlopeAngle = 45.0f;
    m_cfg.walkableHeight = (int)ceilf(2.0f / m_cfg.ch);
    m_cfg.walkableClimb = (int)floorf(0.8f / m_cfg.ch);
    m_cfg.walkableRadius = 0; //(int) ceilf(0.3f / m_cfg.cs);
    m_cfg.maxEdgeLen = (int)(12.0f / 0.3f);
    m_cfg.maxSimplificationError = 1.3f;
    m_cfg.minRegionArea = (int)rcSqr(8); // area = size*size
    m_cfg.mergeRegionArea = (int)rcSqr(20); // area = size*size
    m_cfg.maxVertsPerPoly = (int)6.0f;
    m_cfg.detailSampleDist = 6.0f < 0.9f ? 0 : 0.3f * 6.0f;
    m_cfg.detailSampleMaxError = 0.2f * 1.0f;

    rcVcopy(m_cfg.bmin, bmin);
    rcVcopy(m_cfg.bmax, bmax);
    rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height);

    rcHeightfield* m_solid = rcAllocHeightfield();
    if (!m_solid)
    {
        return -1;
    }
    if (!rcCreateHeightfield(&ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch))
    {
        LOGSVR_ERROR("buildNavigation: Could not create solid heightfield.");
        return -1;
    }

    unsigned char* m_triareas = new unsigned char[ntris];
    if (!m_triareas)
    {
        LOGSVR_ERROR("buildNavigation: Out of memory 'm_triareas' (" << ntris << ")");
        return -1;
    }
    memset(m_triareas, 0, ntris * sizeof(unsigned char));
    rcMarkWalkableTriangles(&ctx, m_cfg.walkableSlopeAngle, verts, nverts, tris, ntris, m_triareas);
    rcRasterizeTriangles(&ctx, verts, nverts, tris, m_triareas, ntris, *m_solid, m_cfg.walkableClimb);
    delete[] m_triareas;
    m_triareas = 0;

    rcFilterLowHangingWalkableObstacles(&ctx, m_cfg.walkableClimb, *m_solid);
    rcFilterLedgeSpans(&ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid);
    rcFilterWalkableLowHeightSpans(&ctx, m_cfg.walkableHeight, *m_solid);

    rcCompactHeightfield* m_chf = rcAllocCompactHeightfield();
    if (!m_chf)
    {
        LOGSVR_ERROR("buildNavigation: Out of memory 'chf'.");
        return -1;
    }
    if (!rcBuildCompactHeightfield(&ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf))
    {
        LOGSVR_ERROR("buildNavigation: Could not build compact data.");
        return -1;
    }

    rcFreeHeightField(m_solid);
    m_solid = 0;

    if (!rcErodeWalkableArea(&ctx, m_cfg.walkableRadius, *m_chf))
    {
        LOGSVR_ERROR("buildNavigation: Could not erode.");
        return -1;
    }

    //这部分可选
    const ConvexVolume* vols = m_geom->getConvexVolumes();
    for (int i = 0; i < m_geom->getConvexVolumeCount(); ++i)
        rcMarkConvexPolyArea(&ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf);

    enum SamplePartitionType
    {
        SAMPLE_PARTITION_WATERSHED,
        SAMPLE_PARTITION_MONOTONE,
        SAMPLE_PARTITION_LAYERS,
    };

    //三种算法,默认选第一种
    int m_partitionType = 0;
    if (m_partitionType == SAMPLE_PARTITION_WATERSHED)
    {
        if (!rcBuildDistanceField(&ctx, *m_chf))
        {
            LOGSVR_ERROR("buildNavigation: Could not build distance field.");
            return -1;
        }

        if (!rcBuildRegions(&ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea))
        {
            LOGSVR_ERROR("buildNavigation: Could not build watershed regions.");
            return -1;
        }
    }
    else if (m_partitionType == SAMPLE_PARTITION_MONOTONE)
    {
        if (!rcBuildRegionsMonotone(&ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea))
        {
            LOGSVR_ERROR("buildNavigation: Could not build monotone regions.");
            return -1;
        }
    }
    else // SAMPLE_PARTITION_LAYERS
    {
        if (!rcBuildLayerRegions(&ctx, *m_chf, 0, m_cfg.minRegionArea))
        {
            LOGSVR_ERROR("buildNavigation: Could not build layer regions.");
            return -1;
        }
    }

    rcContourSet* m_cset = rcAllocContourSet();
    if (!m_cset)
    {
        LOGSVR_ERROR("buildNavigation: Out of memory 'cset'.");
        return -1;
    }
    if (!rcBuildContours(&ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset))
    {
        LOGSVR_ERROR("buildNavigation: Could not create contours.");
        return -1;
    }

    rcPolyMesh* m_pmesh = rcAllocPolyMesh();
    if (!m_pmesh)
    {
        LOGSVR_ERROR("buildNavigation: Out of memory 'pmesh'.");
        return -1;
    }
    if (!rcBuildPolyMesh(&ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh))
    {
        LOGSVR_ERROR("buildNavigation: Could not triangulate contours.");
        return -1;
    }

    rcPolyMeshDetail* m_dmesh = rcAllocPolyMeshDetail();
    if (!m_dmesh)
    {
        LOGSVR_ERROR("buildNavigation: Out of memory 'pmdtl'.");
        return -1;
    }

    if (!rcBuildPolyMeshDetail(&ctx, *m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh))
    {
        LOGSVR_ERROR("buildNavigation: Could not build detail mesh.");
        return -1;
    }

    rcFreeCompactHeightfield(m_chf);
    m_chf = 0;
    rcFreeContourSet(m_cset);
    m_cset = 0;

    //锟节八诧拷: 锟斤拷锟斤拷detour锟斤拷锟斤拷
    enum SamplePolyAreas
    {
        SAMPLE_POLYAREA_GROUND,
        SAMPLE_POLYAREA_WATER,
        SAMPLE_POLYAREA_ROAD,
        SAMPLE_POLYAREA_DOOR,
        SAMPLE_POLYAREA_GRASS,
        SAMPLE_POLYAREA_JUMP,
    };

    CHECK_EXPR(m_cfg.maxVertsPerPoly <= 6, -1, "per poly's verts must <= 6");

    unsigned char* navData = 0;
    int navDataSize = 0;

    for (int i = 0; i < m_pmesh->npolys; ++i)
    {
        if (m_pmesh->areas[i] == 63)
            m_pmesh->areas[i] = SAMPLE_POLYAREA_GROUND;

        if (m_pmesh->areas[i] == SAMPLE_POLYAREA_GROUND || m_pmesh->areas[i] == SAMPLE_POLYAREA_GRASS || m_pmesh->areas[i] == SAMPLE_POLYAREA_ROAD)
        {
            m_pmesh->flags[i] = SAMPLE_POLYFLAGS_WALK;
        }
        else if (m_pmesh->areas[i] == SAMPLE_POLYAREA_WATER)
        {
            m_pmesh->flags[i] = SAMPLE_POLYFLAGS_SWIM;
        }
        else if (m_pmesh->areas[i] == SAMPLE_POLYAREA_DOOR)
        {
            m_pmesh->flags[i] = SAMPLE_POLYFLAGS_WALK | SAMPLE_POLYFLAGS_DOOR;
        }
    }

    dtNavMeshCreateParams params;
    memset(&params, 0, sizeof(params));
    params.verts = m_pmesh->verts;
    params.vertCount = m_pmesh->nverts;
    params.polys = m_pmesh->polys;
    params.polyAreas = m_pmesh->areas;
    params.polyFlags = m_pmesh->flags;
    params.polyCount = m_pmesh->npolys;
    params.nvp = m_pmesh->nvp;
    params.detailMeshes = m_dmesh->meshes;
    params.detailVerts = m_dmesh->verts;
    params.detailVertsCount = m_dmesh->nverts;
    params.detailTris = m_dmesh->tris;
    params.detailTriCount = m_dmesh->ntris;
    params.offMeshConVerts = m_geom->getOffMeshConnectionVerts();
    params.offMeshConRad = m_geom->getOffMeshConnectionRads();
    params.offMeshConDir = m_geom->getOffMeshConnectionDirs();
    params.offMeshConAreas = m_geom->getOffMeshConnectionAreas();
    params.offMeshConFlags = m_geom->getOffMeshConnectionFlags();
    params.offMeshConUserID = m_geom->getOffMeshConnectionId();
    params.offMeshConCount = m_geom->getOffMeshConnectionCount();
    params.walkableHeight = 2.0f;
    params.walkableRadius = 0.0f;
    params.walkableClimb = 0.4f;
    rcVcopy(params.bmin, m_pmesh->bmin);
    rcVcopy(params.bmax, m_pmesh->bmax);
    params.cs = m_cfg.cs;
    params.ch = m_cfg.ch;
    params.buildBvTree = true;

    if (!dtCreateNavMeshData(&params, &navData, &navDataSize))
    {
        LOGSVR_ERROR("Could not build Detour navmesh.");
        return -1;
    }

    CHECK_NULL(0, navData);

    std::string szNavDataName = NF_FORMAT("{}/scene/navmesh/{}.navmesh", pszPath, pszResName);

    FILE* pObjFile = fopen(szNavDataName.c_str(), "w");
    CHECK_NULL(0, pObjFile);
    size_t size = fwrite(navData, 1, navDataSize, pObjFile);
    if (size != (size_t)navDataSize)
    {
        fclose(pObjFile);
        LOGSVR_ERROR("write err. filename=" << szNavDataName);
        return -1;
    }

    fflush(pObjFile);
    fclose(pObjFile);
    return 0;

    dtNavMesh* m_navmesh = dtAllocNavMesh();
    CHECK_NULL(0, m_navmesh);
    dtStatus dtRet = m_navmesh->init(navData, navDataSize, DT_TILE_FREE_DATA);
    if (dtRet != DT_SUCCESS)
    {
        //清除内存
        LOGSVR_ERROR("init m_navmesh wrong. map=" << pszResName);
        dtFreeNavMesh(m_navmesh);
        return -1;
    }
    dtNavMeshQuery* pstNavmeshQuery = dtAllocNavMeshQuery();
    CHECK_NULL(0, pstNavmeshQuery);

    dtRet = pstNavmeshQuery->init(m_navmesh, 2048);
    if (dtRet != DT_SUCCESS)
    {
        //清除内存
        LOGSVR_ERROR("init pstNavmeshQuery wrong. map=" << pszResName);

        dtFreeNavMeshQuery(pstNavmeshQuery);
        dtFreeNavMesh(m_navmesh);
        return -1;
    }

    LOGSVR_TRACE("--end-- path:" << pszPath << ", resname:" << pszResName);

    return 0;
}

int NFNavmeshUtil::ClientNavData2ServerObjData(char* szAllocBuffer, int iMaxBuffSize, int& vertext_num, int& trig_num, tinyxml2::XMLElement* pVertexElement, tinyxml2::XMLElement* pTrigIndexElement, const char* obj_file_name)
{
    vertext_num = 0;
    trig_num = 0;
    CHECK_NULL(0, pVertexElement);
    CHECK_NULL(0, pTrigIndexElement);

    const char* szVertex = pVertexElement->GetText();
    CHECK_EXPR(szVertex, 0, "");
    std::string strVertex = pVertexElement->GetText();
    const char* szTrig = pTrigIndexElement->GetText();
    CHECK_EXPR(szTrig, 0, "");
    std::string strTrig = pTrigIndexElement->GetText();

    //snprintf(obj_filename, sizeof(obj_filename), "%s/mapdata/navmesh/%s.obj", MAYEXDESCSTOREMNG->GetDataFilePath(), res_name);
    FILE* fp = fopen(obj_file_name, "w");
    CHECK_NULL(0, fp);

    int index = 0, vernum = 0;
    const int iBuffSize = 200;
    char szBuffer[iBuffSize];
    for (int i = 0; i < (int)(strVertex.size() + 1); i++)
    {
        if (i == (int)(strVertex.size()))
        {
            int size = i - index;
            if (size > 0)
            {
                memset(szBuffer, 0, sizeof(szBuffer));
                std::string temp = strVertex.substr(index, size);

                if (vernum == 0)
                    sprintf(szBuffer, "v %s", temp.c_str());
                else
                    sprintf(szBuffer, " %s", temp.c_str());
                vernum++;
                if (vernum >= 3)
                {
                    vernum = 0;
                    sprintf(szBuffer, "%s \r\n", szBuffer);
                }
                size_t write_size = fwrite(szBuffer, 1, strlen(szBuffer), fp);
                if (write_size != strlen(szBuffer))
                {
                    fclose(fp);
                    LOGSVR_ERROR("write err. filename=" << obj_file_name);
                    return -1;
                }

                fflush(fp);

                sprintf(szAllocBuffer, "%s%s", szAllocBuffer, szBuffer);
                vertext_num++;
            }
            break;
        }
        switch (*szVertex++)
        {
            case ' ':
            {
                break;
            }
            case ',':
            {
                int size = i - index;
                if (size > 0)
                {
                    memset(szBuffer, 0, sizeof(szBuffer));
                    std::string temp = strVertex.substr(index, size);

                    if (vernum == 0)
                        sprintf(szBuffer, "v %s", temp.c_str());
                    else
                        sprintf(szBuffer, " %s", temp.c_str());
                    vernum++;
                    if (vernum >= 3)
                    {
                        vernum = 0;
                        sprintf(szBuffer, "%s \r\n", szBuffer);
                    }
                    size_t write_size = fwrite(szBuffer, 1, strlen(szBuffer), fp);
                    if (write_size != strlen(szBuffer))
                    {
                        fclose(fp);
                        LOGSVR_ERROR("write err. filename=" << obj_file_name);
                        return -1;
                    }
                    fflush(fp);

                    sprintf(szAllocBuffer, "%s%s", szAllocBuffer, szBuffer);
                    vertext_num++;
                }
                index = i + 1;
                break;
            }
            default:
                break;
        }
    }

    int trignum = 0;
    index = 0;
    for (int i = 0; i < (int)(strTrig.size() + 1); i++)
    {
        if (i == (int)(strTrig.size()))
        {
            int size = i - index;
            if (size > 0)
            {
                memset(szBuffer, 0, sizeof(szBuffer));
                std::string temp = strTrig.substr(index, size);

                int value = atoi(temp.c_str());
                value++;
                if (trignum == 0)
                    sprintf(szBuffer, "f %d", value);
                else
                    sprintf(szBuffer, " %d", value);
                trignum++;
                if (trignum >= 3)
                {
                    trignum = 0;
                    sprintf(szBuffer, "%s \r\n", szBuffer);
                }

                size_t write_size = fwrite(szBuffer, 1, strlen(szBuffer), fp);
                if (write_size != strlen(szBuffer))
                {
                    fclose(fp);
                    LOGSVR_ERROR("write err. filename=" << obj_file_name);
                    return -1;
                }
                fflush(fp);

                sprintf(szAllocBuffer, "%s%s", szAllocBuffer, szBuffer);
                trig_num++;
            }
            break;
        }
        switch (*szTrig++)
        {
            case ',':
            {
                int size = i - index;
                if (size > 0)
                {
                    memset(szBuffer, 0, sizeof(szBuffer));
                    std::string temp = strTrig.substr(index, size);

                    int value = atoi(temp.c_str());
                    value++;
                    if (trignum == 0)
                        sprintf(szBuffer, "f %d", value);
                    else
                        sprintf(szBuffer, " %d", value);
                    trignum++;
                    if (trignum >= 3)
                    {
                        trignum = 0;
                        sprintf(szBuffer, "%s \r\n", szBuffer);
                    }

                    size_t write_size = fwrite(szBuffer, 1, strlen(szBuffer), fp);
                    if (write_size != strlen(szBuffer))
                    {
                        fclose(fp);
                        LOGSVR_ERROR("write err. filename=" << obj_file_name);
                        return -1;
                    }
                    fflush(fp);
                    sprintf(szAllocBuffer, "%s%s", szAllocBuffer, szBuffer);
                    trig_num++;
                }
                index = i + 1;
                break;
            }
            default:
                break;
        }
    }

    return fclose(fp);
}
